package falstad;
import java.util.Random;
import java.util.Scanner;
import java.util.ArrayList;


public class CuriousGamblerDriver implements RobotDriver {
	Scanner reader = new Scanner(System.in);
	private Robot robot;
	
	public CuriousGamblerDriver(Robot r)
	{
		this.robot = r;
	}
	
	@Override
	public void setRobot(Robot r) throws UnsuitableRobotException {
		robot = r;
		float robotBattLevel = robot.getCurrentBatteryLevel();
		float robotRotationCost = robot.getEnergyForFullRotation();
		float robotMoveCost = robot.getEnergyForStepForward();
		if ((robotBattLevel == 2500) && (robotRotationCost == 4) && (robotMoveCost == 5)) {
			try {
				robot.distanceToObstacleAhead();
				robot.distanceToObstacleOnLeft();
				robot.canSeeGoalAhead();
				robot.canSeeGoalOnLeft();
			}
			catch (UnsupportedMethodException e) {
				throw new UnsuitableRobotException();
			}
		}
		
	}

	@Override
	public boolean drive2Exit() throws Exception {
		System.out.println("Battery level: " + robot.getCurrentBatteryLevel());
		int visitedList[][] = new int[robot.getMazeWidth()][robot.getMazeWidth()];
		for (int i=0; i< robot.getMazeWidth(); i++) {
			for (int j=0; j< robot.getMazeWidth(); j++) {
				visitedList[i][j] = 0;
			}
		}
		Random random = new Random();
		boolean justRotatedLeft = false;
		boolean justRotatedRight = false;
		boolean justMovedFwd = false;
		boolean canMoveLeft = false;
		boolean canMoveRight = false;
		boolean canMoveFwd = false;
		boolean canMoveBck = false;
		
		while (!robot.hasStopped() || !robot.isAtGoal()) {
			robot.printCurrentInfo();
			//System.out.println(robot.getCurrentBatteryLevel());
//			for (int i=0; i< robot.getMazeWidth(); i++) {
//				for (int j=0; j< robot.getMazeWidth(); j++) {
//					System.out.print(visitedList[i][j]);
//					System.out.print(" ");
//				}
//				System.out.println();
//			}
			int currentPos[] = robot.getCurrentPosition();
			if (justMovedFwd) {
				visitedList[currentPos[0]][currentPos[1]] += 1;
			}
			
			
//			System.out.println("Type input:");
//			String input = reader.next();
			if (robot.hasStopped()) {
				throw new Exception();
			}
			else if (robot.isAtGoal()) {
				return true;
			}
			
			//start algorithm
			
			if (robot.canSeeGoalOnLeft()) {
				System.out.println("can move LEFT");
				System.out.println("moving LEFT");
				robot.rotate(Constants.LEFT_ROTATE);
				robot.decreaseBatteryLevel(robot.getEnergyForRotation());
				robot.move(1, Constants.forwards);
				robot.decreaseBatteryLevel(robot.getEnergyForStepForward());
				justMovedFwd = true;
			}
			canMoveFwd = (robot.distanceToObstacleAhead() > 0);
			if (justRotatedRight && !canMoveFwd) { //if just rotated right but couldnt move forwards
				if (canMoveFwd) {
					int moveDirection = random.nextInt(2);
					if (moveDirection == 0) { //MOVE FWD
						System.out.println("Can move FWD");
						System.out.println("moving FWD");
						System.out.println("moving: " + 1);
						robot.move(1, Constants.forwards);
						robot.decreaseBatteryLevel(robot.getEnergyForStepForward());
						justMovedFwd = true; //can move back from here
					}
					else { //(moveDirection ==1) //ROTATE
						int rotateDirection = random.nextInt(2);
						if (rotateDirection == 0) { //Rotate RIGHT
							System.out.println("rotating RIGHT here4");
							robot.rotate(Constants.RIGHT_ROTATE);
							robot.decreaseBatteryLevel(robot.getEnergyForRotation());
							justRotatedRight = true;
						}
						else {// if (rotateDirection == 1) { //Rotate RIGHT TWICE
							System.out.println("rotating RIGHT twice");
							robot.rotate(Constants.RIGHT_ROTATE);
							robot.decreaseBatteryLevel(robot.getEnergyForRotation());
							robot.rotate(Constants.RIGHT_ROTATE);
							robot.decreaseBatteryLevel(robot.getEnergyForRotation());
						}
					}
				}
				else { //cant move fwd, Rotate
//					int rotateDirection = random.nextInt(2);
//					if (rotateDirection == 0) { //Rotate RIGHT
//						System.out.println("rotating RIGHT here3");
//						robot.rotate(Constants.RIGHT_ROTATE);
//						robot.decreaseBatteryLevel(robot.getEnergyForRotation());
//						justRotatedRight = true;
//					}
//					else {// if (rotateDirection == 1) { //Rotate RIGHT TWICE
//						System.out.println("rotating RIGHT twice");
//						robot.rotate(Constants.RIGHT_ROTATE);
//						robot.decreaseBatteryLevel(robot.getEnergyForRotation());
//						robot.rotate(Constants.RIGHT_ROTATE);
//						robot.decreaseBatteryLevel(robot.getEnergyForRotation());
//					}
					//TODO
				}
			}
			else {
				canMoveLeft = (robot.distanceToObstacleOnLeft() > 0);
				canMoveFwd = (robot.distanceToObstacleAhead() > 0);
				int leftDistance = robot.distanceToObstacleOnLeft();
				int fwdDistance = robot.distanceToObstacleAhead();
	
				if (canMoveFwd && canMoveLeft) {
					//if (rotated == 4) //full rotation, all spots equal visit count
						//moveDirection = random(2)
						// if (moveDirection == 0)
							//move fwd
						// else // (moveDirection == 1)
							//move left
						//rotated = 0 //reset rotate count
					//else if (visitedList[fwd[0]][fwd[1]] < visitedList[left[0]][left[1]]) //move fwd
						//move fwd
						//rotated = 0
					//else if (visitedList[fwd[0]][fwd[1]] > visitedList[left[0]][left[1]]) //move left
						//move left
						//rotated = 0
					//else //rotate right, try again
						//rotate right
						//rotated ++
				}
			}
		}
		//returns false otherwise
		return false;
	}

	@Override
	public float getEnergyConsumption() {
		return (2500 - robot.getCurrentBatteryLevel());
	}

	@Override
	public int getPathLength() {
		return robot.getDistTraveled();
	}

	
}
